#include <chassis_controller/kinematics.h>
#include <chassis_controller/kinematics_multi_pivots.h>
#include <chassis_controller/kinematics_tricycle.h>
#include <chassis_controller/kinematics_diff.h>
namespace chassis_controller
{
    Kinematics *Kinematics::create(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, int chassis_type)
    {
        if (chassis_type == MULTI_PIVOTS)
        {
            Kinematics *kinematicsPtr = new chassis_controller::MultiPivotsKinematics(node);
            return kinematicsPtr;
        }
        else if (chassis_type == TRICYLE)
        {
            Kinematics *kinematicsPtr = new chassis_controller::TricycleKinematics(node);
            return kinematicsPtr;
        }
        else if (chassis_type == DIFF)
        {
            Kinematics *kinematicsPtr = new chassis_controller::DiffKinematics(node);
            return kinematicsPtr;
        }        
        return nullptr;
    }

}